#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     left,          tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     right,         tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    arm,                  tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

task main()
{
  while(true)
  {
    getJoystickSettings(joystick);


    if(joystick.joy1_y1 < 10 && joystick.joy1_y1 > -10)       // deadzones
    {
      motor[left] = 0;
    }
    else
    {
      motor[left] = joystick.joy1_y1/1.5;      //left motor settings
    }



    if(joystick.joy1_y2 < 10 && joystick.joy1_y2 > -10)       // deadzones
    {
      motor[right] = 0;
    }
    else
    {
      motor[right] = joystick.joy1_y2/1.5;      //right motor settings
    }

    if(joy1Btn(1) == (1)                         //servo up controll
    {
      servo[arm]++;
    }
    else
    {

    }

     if(joy1Btn(2) == (1)                         //servo down controll
    {
      servo[arm]--;
    }
    else
    {

    }



  }
}
